#ifndef CONTROL_H
#define CONTROL_H

#include "utils.h"
#include "common/pid_control.h"

#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"

namespace control 
{

struct Pose_type
{
  double x;
  double y;
  double heading;
  double curvature;
  std::string            rfid;
  int                    id;
  int                    direction_index;
  double max_v;
};

struct PathInfoType
{
    int                    PathId;//路径的ID号
    std::string            PathType;//路径的类型，1：直线，2：曲线
    std::vector<Pose_type> PathPointInfo;//路径所包含到的点信息，直线是起点和终点信息，曲线是二阶贝塞尔曲线（起点、终点和控制点）
    double                 v_max;//路径上可行驶的最大速度
    double                 w_max;//路径上可行驶的最大速度
};

struct ProjectionIntervalType
{
    Pose_type start_point;
    Pose_type end_point;
    Pose_type projection_point;
    int       index_nearest;
};

struct ControlParamType
{
    double stop_dis_tolerance_straight;
    double stop_dis_tolerance_curve;
    double curve_insert_dis;
    double change_control_param_dis;
    double offsety_tolerance;
    double offsety_control_level_1;
    double offsety_control_level_2;
    double offsetangle_tolerance;
    double offsetangle_tolerance1;
    double offsetangle_control_level_1;
    double offsetangle_control_level_2;
    double creep_distance;
    double creep_speed;
    double arrive_target_tolerance;
    double obs_brake_dec;
    double obs_estop_dec;
    double estop_speed_level;
	double actuator_delaytime;
    double actuator_delaytime_1;
    double actuator_delaytime_2;
    double adaptive_kp_line;
    double adaptive_ki_line;
    double adaptive_kd_line;
    double adaptive_kp_curve;
    double adaptive_ki_curve;
    double adaptive_kd_curve;
    double adaptive_kp_heading_line;
    double adaptive_ki_heading_line;
    double adaptive_kd_heading_line;
    double adaptive_kp_heading_curve;
    double adaptive_ki_heading_curve;
    double adaptive_kd_heading_curve;
    double line_offsety;
    double line_offsety_max;
    double curve_offsety;
    double curve_offsety_max;
    double travel_dis_max;
    double start_speed_limit;
    double start_acc_limit;
    double acc_limit;
    double acc_limit_w;
    double start_acc_limit_w;
    double dec_limit;
    int lift_flag;
    double pid_tolerance;
    double integrator_saturation_offsety;
    double integrator_saturation_heading;
    int pid_diff_mode_offsety;
    int pid_diff_mode_heading;
    std::string m_path_info;
    bool endpoint_rotate_enable;
};

class TrajectoryTracking
{
public:

    TrajectoryTracking()  = default;
    ~TrajectoryTracking() = default;

	// 轨迹跟踪： 输入轨迹信息，定位信息，车辆状态信息(当前行驶速度)，输出：速度给定和角速度给定
    TrajectoryTracking(PathInfoType &path, geometry_msgs::msg::Pose2D &vehicle_center, double max_speed, double max_omg, double target_speed, double dec_new,double vehicle_speed, double &vel_cmd,double &w_cmd, double cycle_time, int stop_step,uint8_t stop_obs_flag, int turn_direction, double travel_dis, PIDControl& pid_control_offsety, PIDControl& pid_control_heading,ControlParamType controlParamInfo);
    // 路径生成：直线路径和曲线路径
    void        TrajectoryGenerate();
    Pose_type   getProjectPoint2Line(Pose_type &p1, Pose_type &p2, geometry_msgs::msg::Pose2D &p0);
    double      CalculateCurveLength(int path_index,std::vector<Pose_type> curve_path_points,Pose_type &p1, Pose_type &p2,geometry_msgs::msg::Pose2D &vehicle_center);
    double      getLaneHeading(Pose_type p_start,Pose_type p_end);
    double      OmgCommandPlan_new(double init_angle,double current_angle,double target_angle,double omg_max,double &omg_command,uint8_t stop_obs_flag);
    double      VelocityPlan(std::string path_type,const double &current_target_dis,Pose_type &target_position,double &max_speed, double &targetVelocity,double &vel_cmd,int &stop_step);
    void        CalculateOffset(Pose_type &p1, Pose_type &p2, geometry_msgs::msg::Pose2D &p0,Pose_type &projection_point);
    bool        JudgeInterval(Pose_type &p1, Pose_type &p2, Pose_type &projection);
    double      TravelDistance(std::string path_type,double travel_dis,double offset_y,double current_speed,double cycletime);//add 20210315 路径偏差过大时的停车保护
    std::pair<double,double> PIDController(geometry_msgs::msg::Pose2D &vehicle_center, Pose_type &target_position, double offset_y,double offset_heading,ControlParamType controlParamInfo,double dt,PIDControl& pid_control_offsety,PIDControl& pid_control_heading);
    ProjectionIntervalType CalculateProjectionInterval();

    std::string PathType();
    Pose_type   PathStartPoint() const;
    Pose_type   PathEndPoint() const;
    Pose_type   PathControlPoint() const;
    Pose_type   getProjectionPoint() const;
    double      getOffsetY() const;
    double      getOffsetX() const;
    double      getOffsetHeading() const;
    double      getOffsetSpeed() const;
    double      getVelCmd();
    double      getWCmd();
    double      getW_offsety();
    double      getW_offsetheading();
    double      getTargetDis();
    double      getDecNew() const;
    double      getTravelDistance() const;
    double      getCosTheta() const;
    bool        TrajectoryTracking_complete() const;
    int         getStopStep();
    void        Clear();

private:

    double offset_y_,offset_x_;// 单位：m 描述：横向/纵向路径偏差,车身中心点与路径最近点的横向偏差
    double offset_heading_;	// 单位：deg  描述：航向角偏差
    double offset_speed_;	// 单位：m/s  描述：速度偏差
    double target_dis_;
    double dec_new_;
    Pose_type p1_,p2_;
    Pose_type start_point_,end_point_,control_point_;//轨迹的起点、终点和控制点
    Pose_type projection_;
    geometry_msgs::msg::Pose2D  vehicle_center_;
    std::vector<Pose_type> curve_path_points_;

    uint8_t stop_obs_flag_;
    double  max_speed_;
    double  max_omg_;
    double  target_speed_;
    double  vehicle_speed_;
    double  vel_cmd_;
    double  w_cmd_;
    double  w_offsety_;
    double  w_offsetheading_;
    double  cycle_time_;
    int     stop_step_;
    int     turn_direction_;
    double  offsetangle_;
    double  straightAngle_;
    double  init_angle_;
    double  travel_dis_;
    double  cos_theta_;//add 20210319
    PathInfoType     path_;
    PIDControl       pid_control_offsety_;
    PIDControl       pid_control_heading_;
    ControlParamType controlParamInfo_;
    bool             complete_flag_;
};
}
#endif // CONTROL_INFO_H

